/*************************************************************************
                           test  -  description
                             -------------------
    début                : 13 févr. 2012
    copyright            : (C) 2012 par lthouvenin
*************************************************************************/

//---------- Réalisation de la classe <test> (fichier test.cpp) --

//---------------------------------------------------------------- INCLUDE

//-------------------------------------------------------- Include système
using namespace std;
#include <iostream>
#include <stdio.h>
#include <stdlib.h>

//------------------------------------------------------ Include personnel
#include "test.h"
#include "../config.h"
#include "../util.h"
#include "../Vision/VideoSource.h"
#include "../IA/Strategie.h"
#include "../Controle/ControleRobot.h"

//------------------------------------------------------------- Constantes

//---------------------------------------------------- Variables de classe

//----------------------------------------------------------- Types privés


//------------------------------------------------------ Fonctions de test
void testLigneDroite(const string& comPortTest)
// Test visant a verifier si l'envoi repete de commandes SetVitesseRoues
// a une influence (negative) sur la trajectoire du robot.
// Test de la roation d'un robot
{}
// Resultat du test : Redefinir la vitesse n'effecte pas le deplacement


void testReplacement(const string& comPortTest)
// Test du replacement d'un robot
{
	cout << endl << "[***** Debut de testRotation *****]" << endl << endl;

	// Initialisation
	MsgQueue<DataVision> videoToControl;
	MsgQueue<DataIA> IAToControl_RG;
	list<MsgQueue<DataVision>*> visionQueues;
	visionQueues.push_back(&videoToControl);
	VideoSource vision(LARGEUR_TERRAIN_IMAGE_X, LONGUEUR_TERRAIN_IMAGE_Y, visionQueues);
	vision.InitField();
	cout << vision.LancerAnalyse() << endl;

	usleep(2000000);

	ControleRobot controleRobotTest(comPortTest, ROBOT_GAUCHE, &videoToControl, &IAToControl_RG);
	controleRobotTest.LancerControle();

	DataIA dataIA;
	dataIA.posObjectif[X]=30;
	dataIA.posObjectif[Y]=60;
	dataIA.dirObjectif[X]=0;
	dataIA.dirObjectif[Y]=1;
	dataIA.typeStrategie = ATTAQUE_GAUCHE;
	IAToControl_RG.push(dataIA);

	getchar();

	controleRobotTest.ArreterControle();
	vision.ArreterAnalyse();

	cout << endl << "[***** Fin de testRotation *****]" << endl << endl;
}


void testDistanceEntreRoues(const string& comPortTest)
// Procedure visant a estimer la distance entre les roues :
//	On fait faire X tours au robot et on check la distance parcourue par les roues.
//	On a distanceParcourue = X * distanceEntreRoues * PI.
//	TESTER SUR PLUSIEURS ROBOTS
{
	cout << endl << "[***** Debut de testDistanceEntreRoues *****]" << endl << endl;

	Communication comTest;
	cout << comTest.Ouvrir(comPortTest) << endl;
	cout << comTest.SetCompteurPositionRoues(0, 0) << endl;
	cout << comTest.SetVitesseRoues(-4, 4) << endl;

	getchar(); //On arrete manuellement le robot
	cout << comTest.SetVitesseRoues(0, 0) << endl;
	pair<int, int> positions = comTest.GetPositionRoues(); //positions roues
	cout << "Positions initiales des roues : " << positions.first << ", " << positions.second << endl;

	cout << endl << "[***** Fin de testDistanceEntreRoues *****]" << endl << endl;
}


void testRotationParPosition(const string& comPortTest)
// Test de la rotation d'un robot en utilisant les commandes de positionnement des roues
{
	cout << endl << "[***** Debut de testRotationParPosition *****]" << endl << endl;

	Communication comTest;
	cout << comTest.Ouvrir(comPortTest) << endl;
	cout << comTest.SetCompteurPositionRoues(0, 0) << endl;
	double angle = 20 * M_PI;
	cout << (angle * CONST_ROTATION) << endl;
	int position = (int)(angle * CONST_ROTATION + 0.5); //Position a parcourir par chaque roue pour faire un tour complet (en pulses)
	cout << position << endl;
	cout << comTest.SetPositionRoues(position, -position) << endl;

	cout << endl << "[***** Fin de testRotationParPosition *****]" << endl << endl;
}


void testDeplacementParPosition(const string& comPortTest)
// Test de l'asservisselent interne au robot Khepera
{
	cout << endl << "[***** Debut de testDeplacementParPosition *****]" << endl << endl;

	Communication comTest;
	comTest.Ouvrir(comPortTest);

	pair<int, int> positions; //positions roues
	positions = comTest.GetPositionRoues();
	cout << "Positions initiales des roues : " << positions.first << ", " << positions.second << endl;

//	comTest.ConfigurerControleurPosition(25,64,25,64);
	comTest.SetPositionRoues(3000, 3000);
	usleep(5000000);
	comTest.SetCompteurPositionRoues(0,0);
	positions = comTest.GetPositionRoues();
	cout << "Positions des roues : " << positions.first << ", " << positions.second << endl;
	comTest.SetPositionRoues(-3000, -3000);


//	int time = 0; //duree exproximative en dixieme de seconde
//	while( time < 50 )
//	{
//		usleep(100000);
//		positions = comTest.GetPositionRoues();
//		cout << "Positions des roues : " << positions.first << ", " << positions.second << endl;
//		time++;
//	}

	cout << endl << "[***** Fin de testDeplacementParPosition *****]" << endl << endl;
}

